/* Soft clock implementation */

/* Copyright (C) 2009-2011 David Zanetti
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; version 2 of the License.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with this program; if not, write to the Free Software Foundation, Inc.,
 *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */


#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/atomic.h>
#include <stdio.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>

#include "global.h"
#include <util/delay.h>

#include "clock.h"
#include "serial.h"

/* FIXME: this will eventually be unused */
uint16_t ticklen;

/* internal clock control values */
uint16_t clock_slew; /* ticks to slew by */
uint16_t clock_slew_period; /* portion of 1s to slew for */

/* ISR routine for async clock */
ISR(TIMER2_OVF_vect) {
	/* clock overflowed, increment the mid-layer clock */
	clock_frac = (clock_frac+1) & 0x7f;
	if (!clock_frac) {
		clock_s++;
		global_counters.uptime++;
		flag_clock |= FLAG_CLOCK_TICK;
	}
}

/* initalise the clock */
void clock_init(void) {

#ifdef DEBUG_CLOCK 
	serial0_tx_PGM(PSTR("clock: init\r\n"));
#endif

	/* first, reset soft clock to 0.0 */
	clock_s = 0;
	clock_frac = 0;
	clock_slew = 0;
	clock_slew_period = 0;

	/* turn off interrupts while we mess with the clock */
	TIMSK2 = 0;

	/* enable async clock mode, may corrupt current values */
	ASSR |= (1 << AS2);

	/* Clock runs in normal mode, since it's 8-bit and being wrapped */
	TCCR2A = 0; /* normal mode, no special features */
	TCCR2B = 0; /* no compares, clock disabled */
	TCCR2B |= (1 << CS20); /* ensure the clock source is running */

	/* reset current and compare */
	TCNT2 = 0;
	OCR2A = 0;
	OCR2B = 0;

#ifdef DEBUG_CLOCK
	serial0_tx_PGM(PSTR("clock: waiting for async ready\r\n"));
#endif 

	/* wait for clock ready */
	while ((ASSR & 0x7)) {
		_delay_us(10);
	}

	/* clear interrupt flags */
	TIFR2 = 0;

	/* start interupts */
	TIMSK2 |= (1 << TOIE2);

	// TCCR2B |= (1 << CS20); /* starts timer */

#ifdef DEBUG_CLOCK
	serial0_tx_PGM(PSTR("clock: running\r\n"));
#endif
}

/* align the clock to the global data provided about it's offset */
void clock_align(void) {
	long ticks;

#ifdef DEBUG_CLOCK
	serial0_tx_PGM(PSTR("GPS:     "));
	serial0_tx_dec(clock_s_gps);
	serial0_tx_cr();
	serial0_tx_PGM(PSTR("Local:   "));
	serial0_tx_dec(clock_s_pps);
	serial0_tx_cout(' ');
	serial0_tx_dec(clock_ms_pps);
	serial0_tx_cout(' ');
	serial0_tx_dec(clock_us_pps / 20);
	serial0_tx_cr();
#endif

	/* okay, so if we have a large gap, then we might as well just
	 * hard reset the clock */
	ticks = clock_s_gps - clock_s_pps;
	if (ticks < -10 || ticks > 10) {
		/* yeah just hard reset it */
		clear_flag(flag_clock,FLAG_CLOCK_SYNC); /* just in case */
		/* ensure this is done clean */
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			clock_s = clock_s_gps - 2;
		}
		serial0_tx_PGM(PSTR("clock: reset\r\n"));
		return;
	}

	/* okay, we have less than 10 seconds shift, reduce the offset down
    to the total number of CPU ticks required */
	ticks = (clock_s_gps - clock_s_pps) * (uint32_t) ticklen * 1000L;
	ticks -= (clock_ms_pps * (uint32_t) ticklen);
	ticks -= clock_us_pps;

#ifdef DEBUG_CLOCK
	serial0_tx_PGM(PSTR("clock: ticks required "));
	if (ticks > 0) {
		serial0_tx_dec(ticks);
	} else {
		serial0_tx_cout('-');
		serial0_tx_dec(-ticks);
	}
	serial0_tx_cr();
#endif

	if (ticks < -20000 || ticks > 20000) {
		if (flag_clock & FLAG_CLOCK_SYNC) {
			serial0_tx_PGM(PSTR("clock: lost sync\r\n"));
		}
		clear_flag(flag_clock,FLAG_CLOCK_SYNC); /* we do not have sync at the moment */
	}

	/* okay, we now know how many ticks we have to make up */

	if (ticks < -2000000 || ticks > 2000000) {
		/* adjust ticklen by about 10% to shift */
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			clock_slew_period = 1001; /* run for at least 1 second */
			if (ticks > 0) {
				//   clock_slew = 1000;
				clock_slew = (ticklen - (ticklen / 10));
			} else {
				clock_slew = (ticklen + (ticklen / 10));
			}
			flag_clock |= FLAG_CLOCK_SLEW; /* safe, interrupts are off */
		}
		return;
	}

	if (ticks < -200000 || ticks > 200000) {
		/* we're still quite far off, but let's slow down how much
     shift we're asking for */
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			clock_slew_period = 250; /* only half a second now */
			if (ticks > 0) {
				clock_slew = (ticklen - (ticklen / 100));
			} else {
				clock_slew = (ticklen + (ticklen / 100));
			}
			flag_clock |= FLAG_CLOCK_SLEW;
		}
		return;
	}

	if (ticks < -20000 || ticks > 20000) {
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			clock_slew_period = 250; /* only 100ms of each second will be slewed */
			if (ticks > 0) {
				clock_slew = (ticklen - (ticklen / 1000));
			} else {
				clock_slew = (ticklen + (ticklen / 1000));
			}
			flag_clock |= FLAG_CLOCK_SLEW;
		}
		return;
	}

	/* run free if we're less than 1k ticks away */
	if (ticks >= -2000 && ticks <= 2000) {
		if (!(flag_clock & FLAG_CLOCK_SYNC)) {
			serial0_tx_PGM(PSTR("clock: sync\r\n"));
		}
		clock_update_s = clock_s;
		clock_update_ms = clock_frac;
		clock_update_us = TCNT1;
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			clock_slew_period = 0;
			flag_clock &= ~(FLAG_CLOCK_SLEW);
			flag_clock |= FLAG_CLOCK_SYNC;
		}
		return;
	}

	/* if we are slewing, then try to get down to 1k */
	if (flag_clock & FLAG_CLOCK_SLEW) {
		/* we're between 10k and 1k ticks, if we're more than 2k, slew it down */
		if (ticks < -1000 || ticks > 1000) {
			ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
				clock_slew_period = 10; /* only 50ms of each second will be slewed */
				if (ticks > 0) {
					clock_slew = ticklen - 93;
				} else {
					clock_slew = ticklen + 93;
				}
				flag_clock |= FLAG_CLOCK_SLEW;
			}
			return;
		}
	}

	/* we're not slewing, let it pick up to 2k before slewing again */
	if (ticks < -3000 || ticks > 3000) {
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			clock_slew_period = 10; /* only 50ms of each second will be slewed */
			if (ticks > 0) {
				clock_slew = ticklen - 92;
			} else {
				clock_slew = ticklen + 93;
			}
			flag_clock |= FLAG_CLOCK_SLEW;
		}
		//  serial0_tx_PGM(PSTR("-3k to 3k\r\n"));
		return;
	}

	/* run free, but with flags instact */
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		clock_slew_period = 0;
	}

	return;

}


